/*
 * command.c
 *
 *  Created on: 15/11/2012
 *      Author: julian
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>

#include "command.h"
#include "serializationUtils.h"

int commandAttrInit(command_arg_t *command, uint8_t bytes, uint8_t short_ints,
        uint8_t ints, uint8_t floats, uint8_t doubles) {

    memset(command, 0, sizeof(command_arg_t));

    command->byte_num = bytes;
    if (bytes > 0) {
        command->byte_args = malloc(command->byte_num * sizeof(uint8_t));
    }

    command->short_num = short_ints;
    if (short_ints > 0) {
        command->short_args = malloc(command->short_num * sizeof(short int));
    }

    command->int_num = ints;
    if (ints > 0) {
        command->int_args = malloc(command->int_num * sizeof(int));
    }

    command->float_num = floats;
    if (floats > 0) {
        command->float_args = malloc(command->float_num * sizeof(float));
    }

    command->double_num = doubles;
    if (doubles > 0) {
        command->dbl_args = malloc(command->double_num * sizeof(double));
    }

    return (0);
}

int createMsg(command_arg_t com, struct msg_t* msg) {

    int i = 0, j = 0, k = 0;
    //struct msg_t msg;

    union {
        int16_t v;
        uint8_t c[2];
    } short_int;

    union {
        float f;
        uint8_t c[4];
    } floating;

    union {
        double d;
        uint8_t c[8];
    } doubling;

    if ((msg==NULL)||com.byte_num + com.int_num * sizeof(int)
            + com.short_num * sizeof(short int)
            + com.double_num * sizeof(double)
            + com.float_num * sizeof(float)> MAX_MSG_DATA_LEN) {

        free(com.byte_args);
        free(com.short_args);
        free(com.float_args);
        free(com.int_args);
        free(com.dbl_args);

        errno = EINVAL;
        return -1;
    }

    msg->options = com.opt;
    msg->type = com.command;


#ifdef CMD_DEBUG
    printf("\ncommand: %x", msg->type);
#endif
    uint8_t len = 0;
    for (i = 0; i < com.byte_num; i++) {
        msg->data[i] = com.byte_args[i];
        ++len;

        j++;
    }
    for (i = 0; i < com.short_num; i++) {

        short_int.v = com.short_args[i];

        msg->data[j + (2 * i)] = short_int.c[1];
        ++len;
        msg->data[j + (2 * i) + 1] = short_int.c[0];
        ++len;

#ifdef CMD_DEBUG
        short_int.c[1] = msg->data[j + (2 * i)];
        short_int.c[0] = msg->data[j + (2 * i) + 1];
        printf(" ARG2Sh: %d", short_int.v);
#endif
        j += 2;
    }

    for (i = 0; i < com.int_num; i++) {

        for (k = 0; k < 4; k++) {
            msg->data[j + (4 * i) + k] = com.int_args[i] >> (24 - 8 * k);
            ++len;
        }
#ifdef CMD_DEBUG
        printf(" ARG2I: %d", com.int_args[i]);
#endif
        j += 4;
    }

    for (i = 0; i < com.float_num; i++) {

        floating.f = com.float_args[i];

        for (k = 0; k < 4; k++) {
            msg->data[j + (4 * i) + k] = floating.c[3 - k];
            ++len;
        }
#ifdef CMD_DEBUG
        printf(" ARG2F: %f", floating.f);
#endif
        j += 4;
    }

    for (i = 0; i < com.double_num; i++) {

        doubling.d = com.dbl_args[i];
        for (k = 0; k < 8; k++) {
            msg->data[j + (8 * i) + k] = doubling.c[7 - k];
            ++len;
#ifdef CMD_DEBUG
            doubling.c[7 - k] = msg->data[j + (8 * i) + k];
#endif
        }
#ifdef CMD_DEBUG
        printf(" ARG2D: %f", doubling.d);
#endif
        j += 8;
    }

    msg->len = len;
    if (com.destination != NULL) {
        strcpy(msg->nodeId, com.destination);
    }

    free(com.byte_args);
    free(com.short_args);
    free(com.float_args);
    free(com.int_args);
    free(com.dbl_args);

    return 0;

}

int sendCommand(command_arg_t com) {

    struct msg_t msg;

    if (createMsg(com, &msg) == -1) {
#ifdef CMD_DEBUG
        perror("toMsg");
#endif
        return -1;
    }

    if (msgOutput(msg, com.destination_list)<0){
#ifdef CMD_DEBUG
        perror("msgOutput");
#endif
        return -1;
    }

    return 0;
}

//----------------------------------------------------------------------------------------------------------

//int avanzarDistanciaCM(uint8_t vel, unsigned short int dist, char * id,
//        dest_list_t *list) {
//
//    command_arg_t com;
//    commandAttrInit(&com, 1, 1, 0, 0, 0);
//
//    com.command = FWD_DIST;
//
//    com.byte_args[0] = vel;
//
//    com.short_args[0] = dist;
//
//    com.destination = id;
//    com.destination_list = list;
//
//    if (createMsg(com) == -1) {
//#ifdef CMD_DEBUG
//        perror("create_msg");
//#endif
//        return -1;
//    }
//
//    return (0);
//
//}
//
//int avanzarPulsos(uint8_t vel, short int pulsos, char * id, dest_list_t *list) {
//
//    command_arg_t com;
//
//    commandAttrInit(&com, 1, 1, 0, 0, 0);
//
//    com.command = FWD_PULSE;
//
//    com.byte_args[0] = vel;
//
//    com.short_args[0] = pulsos;
//
//    com.destination = id;
//    com.destination_list = list;
//
//    if (createMsg(com) == -1) {
//#ifdef CMD_DEBUG
//        perror("create_msg");
//#endif
//        return -1;
//    }
//
//    return (0);
//
//}
//
//int girarGrados(uint8_t dir, uint8_t vel, short int grados, char *id,
//        dest_list_t *list) {
//
//    command_arg_t com;
//
//    commandAttrInit(&com, 1, 1, 0, 0, 0);
//
//    com.command = dir;
//
//    com.byte_args[0] = vel;
//
//    if (dir == RIGHT_DEG) {
//        com.short_args[0] = -grados;
//    } else if (dir == LEFT_DEG) {
//        com.short_args[0] = grados;
//    }
//
//    com.destination = id;
//    com.destination_list = list;
//
//    if (createMsg(com) == -1) {
//#ifdef CMD_DEBUG
//        perror("create_msg");
//#endif
//        return -1;
//    }
//
//    return (0);
//
//}
//
//int girarRadianes(uint8_t dir, uint8_t vel, float radianes, char *id,
//        dest_list_t *list) {
//
//    command_arg_t com;
//
//    commandAttrInit(&com, 1, 0, 0, 1, 0);
//
//    com.command = dir;
//
//    com.byte_args[0] = vel;
//
//    if (dir == RIGHT_RAD) {
//        com.float_args[0] = -(radianes);
//    } else if (dir == LEFT_RAD) {
//        com.float_args[0] = radianes;
//    }
//
//    com.destination = id;
//    com.destination_list = list;
//
//    if (createMsg(com) == -1) {
//#ifdef CMD_DEBUG
//        perror("create_msg");
//#endif
//        return -1;
//    }
//
//    return 0;
//
//}
//
//int avanzar(uint8_t vel, char *id, dest_list_t *list) {
//
//    command_arg_t com;
//
//    commandAttrInit(&com, 1, 0, 0, 0, 0);
//
//    com.command = FWD;
//
//    com.byte_args[0] = vel;
//
//    com.destination = id;
//    com.destination_list = list;
//
//    if (createMsg(com) == -1) {
//#ifdef CMD_DEBUG
//        perror("create_msg");
//#endif
//        return -1;
//    }
//
//    return 0;
//
//}
//
//int retroceder(uint8_t vel, char *id, dest_list_t *list) {
//
//    command_arg_t com;
//
//    commandAttrInit(&com, 1, 0, 0, 0, 0);
//
//    com.command = BWD;
//
//    com.byte_args[0] = vel;
//
//    com.destination = id;
//    com.destination_list = list;
//
//    if (createMsg(com) == -1) {
//#ifdef CMD_DEBUG
//        perror("create_msg");
//#endif
//        return -1;
//    }
//
//    return 0;
//
//}
//
//int detenerse(char *id, dest_list_t *list) {
//
//    command_arg_t com;
//
//    commandAttrInit(&com, 0, 0, 0, 0, 0);
//
//    com.command = STOP;
//
//    com.destination = id;
//    com.destination_list = list;
//
//    if (createMsg(com) == -1) {
//#ifdef CMD_DEBUG
//        perror("create_msg");
//#endif
//        return -1;
//    }
//
//    return 0;
//
//}
//
//uint8_t* barridoUS(char *id, dest_list_t *list) {
//
//    command_arg_t com;
//
//    commandAttrInit(&com, 0, 0, 0, 0, 0);
//
//    com.command = SWEEP;
//
//    com.destination = id;
//    com.destination_list = list;
//
//    com.opt|=RESPONSE_REQUIRED_BIT;
//
//    if (createMsg(com) == -1) {
//#ifdef CMD_DEBUG
//        perror("create_msg");
//#endif
//        return NULL;
//    }
//
//    uint8_t *ret = NULL;
//    ret = getResponse();
//    if (ret == NULL) {
//#ifdef CMD_DEBUG
//        perror("get_response");
//#endif
//    }
//
//    return ret;
//
//}
//
//short int sensorUSobtenerMedidaCM(char *id, dest_list_t *list) {
//
//    command_arg_t com;
//
//    commandAttrInit(&com, 0, 0, 0, 0, 0);
//
//    com.command = READ;
//
//    com.destination = id;
//    com.destination_list = list;
//
//    com.opt|=RESPONSE_REQUIRED_BIT;
//
//    if (createMsg(com) == -1) {
//#ifdef CMD_DEBUG
//        perror("create_msg");
//#endif
//        return -1;
//    }
//
//    uint8_t *ret = NULL;
//    ret = getResponse();
//    if (ret == NULL) {
//#ifdef CMD_DEBUG
//        perror("get_response");
//#endif
//        return -1;
//    } else {
//        unsigned short int val = deSerializeShort(ret);
//        printf("\nobtenerUS %d\n", val);
//        return val;
//    }
//
//}
//
//int foo_cmd(uint8_t cmd, int arg1, int arg2, int arg3, int arg4, char *id,
//        dest_list_t *list) {
//
//    command_arg_t com;
//
//    commandAttrInit(&com, 0, 0, 4, 0, 0);
//
//    com.command = cmd;
//
//    com.int_args[0] = arg1;
//    com.int_args[1] = arg2;
//    com.int_args[2] = arg3;
//    com.int_args[3] = arg4;
//
//    com.destination = id;
//    com.destination_list = list;
//
//    if (createMsg(com) == -1) {
//#ifdef CMD_DEBUG
//        perror("create_msg");
//#endif
//        return -1;
//    }
//
//    return 0;
//
//}
//
//int foo_dbl(uint8_t cmd, int arg1, int arg2, double arg3, char *id,
//        dest_list_t *list) {
//
//    command_arg_t com;
//
//    commandAttrInit(&com, 0, 0, 2, 0, 1);
//
//    com.command = cmd;
//
//    com.int_args[0] = arg1;
//    com.int_args[1] = arg2;
//
//    com.dbl_args[0] = arg3;
//
//    com.destination = id;
//    com.destination_list = list;
//
//    if (createMsg(com) == -1) {
//#ifdef CMD_DEBUG
//        perror("create_msg");
//#endif
//        return -1;
//    }
//
//    return 0;
//
//}

//void comander() {
//
//    char input[20];
//    char c[1];
//    char prev;
//    char buf[50][20];
//    char *id = input;
//    dest_list_t *a = NULL;
//    dest_list_t b;
//    uint8_t i = 0;
//
//    printf("\nWho? (o 'b' para broadcast, 'm' para multicast): ");
//    scanf("%s", input);
//
//    if (strcmp(input, "m") == 0) {
//
//        i = 0;
//        printf("\nId de cada integrante del grupo (0 para finalizar).");
//
//        do {
//
//            printf("\nId %d: ", i);
//            scanf("%s", buf[i]);
//
//        } while (buf[i++][0] != '0');
//
//        b.size = i - 1;
//        id = NULL;
//        a = &b;
//    } else if (strcmp(input, "b") == 0) {
//        strcpy(id, "broadcast");
//    }
//
//    char *d[b.size];
//
//    if (a != NULL) {
//
//        for (i = 0; i < b.size; i++) {
//            d[i] = malloc(strlen(buf[i]) * sizeof(char));
//            strcpy(d[i], buf[i]);
//        }
//
//        b.id_list = d;
//    }
//
//    console_io_open();
//    printf("\nESC para salir.\n ");
//    //-- Main loop
//    do {
//        //-- if there is a key pressed by the user
//        if (console_io_kbhit()) {
//
//            //-- Read the character
//            c[0] = console_io_getch();
//
//            //-- Send the character to the serial port, if different from ESC
//            //if (c[0]!=ESC && c[0]!=prev) {
//            if (c[0] != ESC) {
//
//                switch (c[0]) {
//
//                case FWD: {
//                    avanzar(80, id, a);
//                    break;
//                }
//                case FWD_DIST: {
//                    avanzarDistanciaCM(80, 40, id, a);
//                    break;
//                }
//                case FWD_PULSE: {
//                    avanzarPulsos(80, 40, id, a);
//                    break;
//                }
//                case RIGHT_DEG: {
//                    girarGrados(RIGHT_DEG, 60, 30, id, a);
//                    break;
//                }
//                case LEFT_DEG: {
//                    girarGrados(LEFT_DEG, 60, 30, id, a);
//                    break;
//                }
//                case RIGHT_RAD: {
//                    girarRadianes(RIGHT_RAD, 40, 0.786000, id, a);
//                    break;
//                }
//                case LEFT_RAD: {
//                    girarRadianes(LEFT_RAD, 40, 0.786000, id, a);
//                    break;
//                }
//                case BWD: {
//                    retroceder(80, id, a);
//                    break;
//                }
//                case STOP: {
//                    detenerse(id, a);
//                    break;
//                }
//                case SWEEP: {
//                    uint8_t *ret = barridoUS(id, a);
//                    if (ret != NULL) {
//                        printf("\n\nlectura: ");
//                        for (i = 0; i < 10; i++) {
//                            printf("%d ", deSerializeShort(&ret[2 * i]));
//                        }
//                        printf("\n");
//                    }
//                    break;
//                }
//                case READ: {
//                    int16_t ret = 0;
//                    ret=sensorUSobtenerMedidaCM(id, a);
//                    if (ret > 0 ) {
//                        printf("\n\nlectura: %d\n",ret);
//                    }
//                    break;
//                }
//
//                case 0x75:
//                    updateNodeList();
//                    break;
//
//                default:
//                    break;
//                }
//                prev = c[0];
//                //usleep(100000);
//            }
//        }
//    } while (c[0] != ESC);
//
//    console_io_close();
//
//    if (a != NULL) {
//        for (i = 0; i < b.size; i++) {
//            free(d[i]);
//        }
//    }
//}

